//
// Created by 刘昱杉2020 on 2023/4/17.
//
#include <stdio.h>
#include "test.h"
#include "motor.h"
#include "usart.h"
#include "adv.h"
#include "oled.h"
#include "oled_font.h"
#include "detect.h"
#include "sonar.h"
#include "imu.h"


#include "string.h"
#include "stdbool.h"
ADV test_adv;

;

void test_all()
{
    test_for_oled();
    test_for_uart1();
#ifdef TEST_DETECT
    test_for_detect();
#endif
#ifdef  TEST_DIG_SERVO
    test_for_digital_servo();
#endif
#ifdef TEST_BUS_SERVO
    test_for_bus_servo();
#endif
#ifdef TEST_MOTOR
    //...
#endif
#ifdef TEST_SONAR
    test_for_sonar();
#endif
    //Test-Git_on_Clion
//    led_state();
    HAL_Delay(100);
}


void test_for_bus_servo()
{
    uint8_t servo_status=Motor_bus_servo_setAngle(&test_adv.motors->bus_servo[1],Motor_bus_servo_setID(2),90);
    if(servo_status)
        HAL_UART_Transmit(&huart2,"success_bus", strlen("success_bus"),0xfffff);
}
void test_for_digital_servo()
{
    uint8_t servo_status = Motor_digital_servo_setAngle(&test_adv.motors->digital_servo,90);
    if(servo_status)
        OLED_ShowStr(21,8,1,"angle:");

}

void test_for_oled()
{
    uint8_t value = 42;
//    OLED_ShowStr(31,2,2,"Hello  World");
    OLED_ShowStr(21,1,1,"Value = %d",value);
    HAL_Delay(200);

}

void test_for_uart1()
{
    uint8_t tx_buffer[] = "Hello, UART!\r\n";
    uint8_t rx_buffer[32];

    HAL_UART_Transmit(&huart1, tx_buffer, strlen((const char *)tx_buffer), HAL_MAX_DELAY);
    if (HAL_UART_Receive(&huart1, rx_buffer, sizeof(rx_buffer) - 1, 1000) == HAL_OK) {
        HAL_UART_Transmit(&huart1, rx_buffer, strlen((const char *) rx_buffer), HAL_MAX_DELAY);
    }
}
void test_for_detect()
{
    const char  *status = Detect_GetState();
    //OLED_refresh_line(4,5);

    OLED_ShowStr(21,5,1,"state:%s",status);
    HAL_Delay(200);
    OLED_ClearLine(5);
}
void test_for_sonar()
{
    float distance = HCSR04_GetDistance();
    OLED_ShowStr(21,7,1,"Distance:%.2f",distance);
    HAL_Delay(200);
    OLED_ClearLine(7);
}

/*!
 * 表示程序处于运行状态
 */
void led_state()
{
    HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_9);
    //HAL_Delay(500);
}
void test_all_1()
{

    const char  *status = Detect_GetState();
    OLED_ShowStr(21,5,1,"state:%s",status);
    double distance = HCSR04_GetDistance();
    OLED_ShowStr(21,6,1,"Distance:%.2lf",distance);
    uint16_t angle = 90;
    uint8_t servo_status = Motor_digital_servo_setAngle(&test_adv.motors->digital_servo,angle);
    if(servo_status)
        OLED_ShowStr(21,7,1,"angle:%d",angle);
    JY61P_BlockingUpdate();
    JY61P_PrintData(&data);
    HAL_Delay(150);

    OLED_CLS();


}


